///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#include <buola/cv/ccloud.h>
#include <buola/io/cfile.h>
#include <buola/zz/zreal.h>
#include <buola/zz/if.h>
#include <buola/zz/repeat.h>
#include <buola/zz/operators.h>
#include <buola/functors/predicates/char.h>
#include <buola/image/io.h>
#include <buola/mat/transformations.h>
#include <buola/mat/eigen.h>

namespace buola { namespace cvi {

static CCloud_rgb8 load_cloud_crd(const io::CURI &pURI)
{
    io::CTextReader lF(pURI);
    CCloud_rgb8 lCloud;
    
    int lCap;
    lF >> lCap;

    lCloud.resize(lCap);

    for(int i=0;i<lCap;i++)
    {
        float lDummy;
        auto &lP=lCloud[i];
        
        lF >> lP.mPoint.x() >> lP.mPoint.y() >> lP.mPoint.z() >> lDummy >> lDummy 
           >> lP.mPixel[0] >> lP.mPixel[1] >> lP.mPixel[2];
    }

    return lCloud;
}

struct ARSSParser
{
    ARSSParser(CCloud_rgb8 &pCloud)
        :   mCloud(pCloud)
    {}
    
    void operator()(const std::array<float,12> &pArray) const
    {
        CCloud_rgb8::TPoint lP;
        lP.mPoint.x()=pArray[0]*0.001f;
        lP.mPoint.y()=pArray[1]*0.001f;
        lP.mPoint.z()=pArray[2]*0.001f;

        lP.mPixel[0]=uint8_t(pArray[3]);
        lP.mPixel[1]=uint8_t(pArray[4]);
        lP.mPixel[2]=uint8_t(pArray[5]);

        mCloud.SetOrigin({pArray[7],pArray[8],pArray[9]});
        mCloud.push_back(lP);
    }

    CCloud_rgb8 &mCloud;
};

static CCloud_rgb8 load_cloud_rss(const io::CURI &pURI)
{
    ssize_t lFileSize=io::CFile(pURI).Size();
    char *lData=new char[lFileSize];
    io::PStream lS=io::open(pURI,io::EMode::READ);
    lS->Read((uint8_t*)lData,lFileSize);

    CCloud_rgb8 lCloud;

    char *lB=lData,*lE=lData+lFileSize;
    zz::phrase_parse(lB,lE,*(zz::repeat<12>(zz::ZReal<float>())++[ARSSParser(lCloud)]),zz::if_true(fn::is_ascii_space()));
    
    delete[] lData;

    lCloud.SetOrigin(lCloud.Origin()*0.001f);
    return lCloud;
}

CCloud_rgb8 load_cloud(const io::CURI &pURI)
{
    if(pURI.Extension()=="rss")
        return load_cloud_rss(pURI);
    if(pURI.Extension()=="crd")
        return load_cloud_crd(pURI);
    if(pURI.Extension()=="pcd")
        return load_cloud_pcd(pURI);
    throw XInvalid("unknown extension in load_cloud");
}

void save_cloud(const CCloud_rgb8 &pCloud,const io::CURI &pURI)
{
    if(pURI.Extension()=="pcd")
        return save_cloud_pcd(pCloud,pURI);
    throw XInvalid("unknown extension in load_cloud");
}

void save_cloud(const CCloud_f &pCloud,const io::CURI &pURI)
{
    if(pURI.Extension()=="pcd")
        return save_cloud_pcd(pCloud,pURI);
    throw XInvalid("unknown extension in load_cloud");
}

CCloud_rgb8 load_cloud(const io::CURI &pDepth,const io::CURI &pImage,const io::CURI &pInfo)
{
    auto lDepth=img::load<img::CImage_gray32f>(pDepth);
    auto lColor=img::load<img::CImage_rgb8>(pImage);

    io::CTextReader lInfo(pInfo);

    float lHFOV,lVFOV;
    
    mat::CVec3f lPos;
    mat::C3DRotation lRot;

    lInfo >> lHFOV >> lVFOV >> lPos.x() >> lPos.y() >> lPos.z() >> lRot.a >> lRot.b >> lRot.c;

    CCloud_rgb8 lCloud(lPos);

    float lF=lDepth.Width()/tan(lHFOV/2);

    float lCX=lDepth.Width()/2.0;
    float lCY=lDepth.Height()/2.0;
    
    for(int x=0;x<lDepth.Width();x++)
    {
        for(int y=0;y<lDepth.Height();y++)
        {
            float d=lDepth(x,y);
            if(d==0.0f||d>10.0f) continue;
            CCloud_rgb8::TPoint lP;
            lP.mPoint.y()=d*(x-lCX)/lF;
            lP.mPoint.z()=-d*(y-lCY)/lF;
            lP.mPoint.x()=d;
            lP.mPoint+=lPos;
            lP.mPixel=lColor(x,y);
            lCloud.push_back(lP);
        }
    }

    return lCloud;
}

CCloud_rgb8 make_cloud(const img::CView_xyz32f &pPos,const img::CView_rgb8 &pRGB,const mat::CVec3f &pCamPos)
{
    CCloud_rgb8 lCloud(pCamPos);
    lCloud.reserve(pPos.Width()*pPos.Height());

    for(int y=0;y<pPos.Height();y++)
    {
        auto lPosIt=pPos.Row(y).begin();
        auto lRGBIt=pRGB.Row(y).begin();
        for(int x=0;x<pPos.Width();x++)
        {
            if(is_nan(lPosIt[x][0])) continue;
            lCloud.emplace_back(mat::CVec3f(lPosIt[x][0],lPosIt[x][1],lPosIt[x][2]),lRGBIt[x]);
        }
    }

    return lCloud;
}

CCloud_rgb8 make_cloud(const img::CView_gray32f &pDepth,const img::CView_rgb8 &pImage,const img::CCameraParameters &pCamera)
{
    CCloud_rgb8 lCloud;

    float lFX=pCamera.GetF().x;
    float lFY=pCamera.GetF().y;

    float lCX=pCamera.GetC().x;
    float lCY=pCamera.GetC().y;
    
    for(int x=0;x<pDepth.Width();x++)
    {
        for(int y=0;y<pDepth.Height();y++)
        {
            float d=pDepth(x,y);
            if(d<0||is_nan(d)) continue;
            CCloud_rgb8::TPoint lP;
            lP.mPoint.x()=d*(x-lCX)/lFX;
            lP.mPoint.y()=d*(y-lCY)/lFY;
            lP.mPoint.z()=d;
            lP.mPixel=pImage(x,y);
            lCloud.push_back(lP);
        }
    }

    return lCloud;
}

CCloud_rgb8 make_cloud(const img::CView_gray32f &pDepth,const img::CColor &pColor,const img::CCameraParameters &pCamera)
{
    CCloud_rgb8 lCloud;

    float lFX=pCamera.GetF().x;
    float lFY=pCamera.GetF().y;

    float lCX=pCamera.GetC().x;
    float lCY=pCamera.GetC().y;

    img::CPixel_rgb8 lPixel(pColor);
    
    for(int x=0;x<pDepth.Width();x++)
    {
        for(int y=0;y<pDepth.Height();y++)
        {
            float d=pDepth(x,y);
            if(d<0||is_nan(d)) continue;
            CCloud_rgb8::TPoint lP;
            lP.mPoint.x()=d*(x-lCX)/lFX;
            lP.mPoint.y()=d*(y-lCY)/lFY;
            lP.mPoint.z()=d;
            lP.mPixel=lPixel;
            lCloud.push_back(lP);
        }
    }

    return lCloud;
}

CCloud_n_rgb8 make_cloud_with_normals(const img::CView_gray32f &pDepth,const img::CView_rgb8 &pImage,
                                      const img::CCameraParameters &pCamera,int pRadius)
{
    CCloud_n_rgb8 lCloud;
    auto lPos=make_pos_image(pDepth,pCamera);

    for(int y=pRadius;y<lPos.Height()-pRadius;y++)
    {
        for(int x=pRadius;x<lPos.Width()-pRadius;x++)
        {
            if(is_nan(lPos(x,y)[0])) continue;
            //accumulate over points in the neighbourhood
            float lAcc[9]={0,0,0,0,0,0,0,0,0};
            int lCount=0;
            for(int lDX=-pRadius;lDX<=pRadius;lDX++)
            {
                for(int lDY=-pRadius;lDY<=pRadius;lDY++)
                {
                    float lX=lPos(x+lDX,y+lDY)[0];
                    if(is_nan(lX)) continue;
                    float lY=lPos(x+lDX,y+lDY)[1];
                    float lZ=lPos(x+lDX,y+lDY)[2];
                    lAcc[0] += lX * lX;
                    lAcc[1] += lX * lY;
                    lAcc[2] += lX * lZ;
                    lAcc[3] += lY * lY; // 4
                    lAcc[4] += lY * lZ; // 5
                    lAcc[5] += lZ * lZ; // 8
                    lAcc[6] += lX;
                    lAcc[7] += lY;
                    lAcc[8] += lZ;
                    lCount++;
                }
            }
            if(lCount<3) continue;
            for(float &a : lAcc)
            {
                a/=lCount;
            }
            //compute covariance matrix
            mat::CMat3f lCov;
            lCov(0,0)=lAcc[0]-sq(lAcc[6]);
            lCov(0,1)=lCov(1,0)=lAcc[1]-lAcc[6]*lAcc[7];
            lCov(0,2)=lCov(2,0)=lAcc[2]-lAcc[6]*lAcc[8];
            lCov(1,1)=lAcc[3]-sq(lAcc[7]);
            lCov(1,2)=lCov(2,1)=lAcc[4]-lAcc[7]*lAcc[8];
            lCov(2,2)=lAcc[5]-sq(lAcc[8]);
            //compute smallest eigenvalue and eigenvector
            float lScale=max(abs(lCov));
            lCov=lCov/lScale;
            mat::CVec3f lEigenValues=mat::compute_roots(lCov);
            mat::diag(lCov)-=lEigenValues[0];
            mat::CVec3f lV1=mat::cross(lCov(0,nAll),lCov(1,nAll));
            mat::CVec3f lV2=mat::cross(lCov(0,nAll),lCov(2,nAll));
            mat::CVec3f lV3=mat::cross(lCov(1,nAll),lCov(2,nAll));
            float lL1=mat::norm2(lV1);
            float lL2=mat::norm2(lV2);
            float lL3=mat::norm2(lV3);
            
            mat::CVec3f lNorm;
            if(lL1>lL2&&lL1>lL3)
            {
                lNorm=lV1/sqrt(lL1);
            }
            else if(lL2>lL1&&lL2>lL3)
            {
                lNorm=lV2/sqrt(lL2);
            }
            else
            {
                lNorm=lV3/sqrt(lL3);
            }
            
            CCloud_n_rgb8::TPoint lP;
            lP.mPoint.x()=lPos(x,y)[0];
            lP.mPoint.y()=lPos(x,y)[1];
            lP.mPoint.z()=lPos(x,y)[2];
            lP.mNormal=lNorm;
            lP.mPixel=pImage(x,y);
            lCloud.push_back(lP);
        }
    }

    return lCloud;
/*
    CCloud_n_rgb8 lCloud;

    float lFX=pCamera.GetF().x;
    float lFY=pCamera.GetF().y;

    float lCX=pCamera.GetC().x;
    float lCY=pCamera.GetC().y;
    
    for(int y=1;y<pDepth.Height()-1;y++)
    {
        for(int x=1;x<pDepth.Width()-1;x++)
        {
            float d=pDepth(x,y);
            if(d<0||is_nan(d)) continue;
            float lR=pDepth(x+2,y+2);
            if(is_nan(lR)) continue;
            float lL=pDepth(x-2,y-2);
            if(is_nan(lL)) continue;
            float lU=pDepth(x+2,y-2);
            if(is_nan(lU)) continue;
            float lD=pDepth(x-2,y+2);
            if(is_nan(lD)) continue;
            
            mat::CVec3f lN1=cross(mat::CVec3f(2*lR,2*lR,lFX*(lR-d)),mat::CVec3f(2*lU,-2*lU,lFY*(lU-d)));
            mat::CVec3f lN2=cross(mat::CVec3f(-2*lL,-2*lL,lFX*(lL-d)),mat::CVec3f(-2*lD,2*lD,lFY*(lD-d)));
            
//             if(e=pDepth(x+1,y),!is_nan(e))
//             {
//                 v=mat::CVec3f(e,0,lFX*(e-d));
//             }
// //            else if(e=pDepth(x-1,y),!is_nan(e))
// //            {
// //                v=mat::CVec3f(-e,0,lFX*(e-d));
// //            }
//             else
//             {
//                 continue;
//             }
//             
//             if(e=pDepth(x,y+1),!is_nan(e))
//             {
//                 w=mat::CVec3f(0,e,lFY*(e-d));
//             }
// //            else if(e=pDepth(x,y-1),!is_nan(e))
// //            {
// //                w=mat::CVec3f(0,-e,lFY*(e-d));
// //            }
//             else
//             {
//                 continue;
//             }
//             
            CCloud_n_rgb8::TPoint lP;
            lP.mPoint.x()=d*(x-lCX)/lFX;
            lP.mPoint.y()=d*(y-lCY)/lFY;
            lP.mPoint.z()=d;
            lP.mNormal=lN1/(2*norm(lN1))+lN2/(2*norm(lN2));
//            lP.mNormal=lP.mNormal/norm(lP.mNormal);
//            msg_info() << lP.mNormal.T();
            lP.mPixel=pImage(x,y);
            lCloud.push_back(lP);
        }
    }

    return lCloud;
*/
}

CImage_point_rgb8 make_point_image(const img::CView_gray32f &pDepth,const img::CView_rgb8 &pRGB,
                                        const img::CCameraParameters &pCamera)
{
    CImage_point_rgb8 lReturn(pDepth.Size());

    float lFX=pCamera.GetF().x;
    float lFY=pCamera.GetF().y;

    float lCX=pCamera.GetC().x;
    float lCY=pCamera.GetC().y;
    
    for(int x=0;x<pDepth.Width();x++)
    {
        for(int y=0;y<pDepth.Height();y++)
        {
            float d=pDepth(x,y);
            if(d<0||is_nan(d))
            {
                lReturn(x,y)->mPoint.x()=NAN;
                lReturn(x,y)->mPoint.y()=NAN;
                lReturn(x,y)->mPoint.z()=NAN;
            }
            else
            {
                lReturn(x,y)->mPoint.x()=d*(x-lCX)/lFX;
                lReturn(x,y)->mPoint.y()=d*(y-lCY)/lFY;
                lReturn(x,y)->mPoint.z()=d;
            }
            lReturn(x,y)->mPixel=pRGB(x,y);
        }
    }

    return lReturn;
}

img::CImage_xyz32f make_pos_image(const img::CView_gray32f &pDepth,const img::CCameraParameters &pCamera)
{
    img::CImage_xyz32f lReturn(pDepth.Size());

    float lFX=pCamera.GetF().x;
    float lFY=pCamera.GetF().y;

    float lCX=pCamera.GetC().x;
    float lCY=pCamera.GetC().y;
    
    for(int x=0;x<pDepth.Width();x++)
    {
        for(int y=0;y<pDepth.Height();y++)
        {
            float d=pDepth(x,y);
            if(d<0||is_nan(d))
            {
                lReturn(x,y)[0]=NAN;
                lReturn(x,y)[1]=NAN;
                lReturn(x,y)[2]=NAN;
            }
            else
            {
                lReturn(x,y)[0]=d*(x-lCX)/lFX;
                lReturn(x,y)[1]=d*(y-lCY)/lFY;
                lReturn(x,y)[2]=d;
            }
        }
    }

    return lReturn;
}

/*namespace cvi*/ } /*namespace buola*/ }
